import rclpy
from rclpy.node import Node
import sys, select, termios, tty
from geometry_msgs.msg import Quaternion

settings = termios.tcgetattr(sys.stdin)

#lift, extend, rotate
movingBindings = {
    'w': (0, 1, 0, 0), #EXTEND FOWRAWD
    's': (0, -1, 0, 0), #EXTEND BACK
    'a': (0, 0, 1, 0), #ROTATE LEFT
    'd': (0, 0, -1, 0), #ROTATE RIGHT
    'u': (1, 0, 0, 0), #RISE UP
    'j': (-1, 0, 0, 0), #RISE DOWN
    'i': (0, 0, 0, 1), #LIFT UP
    'k': (0, 0, 0, -1), #LIFT DOWN
}

def getKey():
    tty.setraw(sys.stdin.fileno())
    select.select([sys.stdin], [], [], 0)
    key = sys.stdin.read(1)
    termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings)
    return key

class TeleopKeyboard(Node):

    def __init__(self):
        super().__init__('teleop_keyboard')
        self.publisher_ = self.create_publisher(Quaternion, '/cranebot/move', 10)
        timer_period = 0.1
        self.timer = self.create_timer(timer_period, self.timer_callback)
    
    def timer_callback(self):
        msg = Quaternion()
        key = getKey()
        if key in movingBindings.keys():
            msg.w = float(movingBindings[key][0])
            msg.x = float(movingBindings[key][1])
            msg.y = float(movingBindings[key][2])
            msg.z = float(movingBindings[key][3])
        else:
            msg.x = float(0)
            msg.y = float(0)
            msg.z = float(0)
            msg.w = float(0)
        self.publisher_.publish(msg)
        self.get_logger().info(str(msg))

def main(args=None):
    rclpy.init(args=args)
    teleop_keyboard = TeleopKeyboard()
    rclpy.spin(teleop_keyboard)
    teleop_keyboard.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()





